


#include "SocketMatTransmissionClient.h"
#include <sys/time.h>
#include "mavlink.h"

SocketTransmissionClient SocketMat;//

//析构函数
SocketTransmissionClient::SocketTransmissionClient(void)
{

}

SocketTransmissionClient::~SocketTransmissionClient(void)
{

}

//连接服务器
int SocketTransmissionClient::SocketConnect(const char* ip, int port)
{

    int ret;    
    int on = 1;  
    char sbuf[50];//发送 
    
	//int val = sizeof(struct sockaddr);// 
    socklen_t val = sizeof(struct sockaddr_in);//socklen_t和int应该具有相同的长度
	
// 设置地址信息, ip信息    
    server_addr.sin_family = AF_INET;//sin_family表示协议簇, 一般用AF_INET表示TCP/IP协议
    server_addr.sin_port = htons(port);// 端口号, 要和服务端绑定的一样  
	//sin_addr是一个联合体，用联合体就可以使用多种方式表示IP地址
    server_addr.sin_addr.s_addr = inet_addr(ip);//云服务器的公网ip
	
    //创建UDP的套接字    
    client_sockfd = socket(AF_INET, SOCK_DGRAM,0);//SOCK_DGRAM表示默认使用UDP, 0表示默认 （SOCK_STREAM默认使用TCP）   
    if(client_sockfd<0)    
    {    
        perror("failed socket");    
        return -1;    
    }  
	
    //设置端口复用  
	//SOL_SOCKET 用于setsockopt/getsockopt函数, 表示设置（获取）socket参数, SOL应该是socket level的缩写	
    setsockopt(client_sockfd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on));     
  
/*	//发送信息给服务器 
	puts("please enter data:");    
	cin>>sbuf;    
	ret=sendto(client_sockfd, sbuf, sizeof(sbuf), 0, (struct sockaddr*)&server_addr, val);    
	if(ret<0)    
	{    
		perror("sendto failed");    
		return -1;
	}    
*/	//bzero(sbuf, sizeof(sbuf));//清零 

	return 1;
}

//与服务器断开连接
void SocketTransmissionClient::SocketDisconnect(void)
{
	//close(sockClient);
}

//
//
int SocketTransmissionClient::Transmit_Server(cv::Mat image)
{
	
	int ret;
	char encodeImg[65535];//编码数组
	socklen_t val = sizeof(struct sockaddr);
	
	std::vector<uchar> data_encode;//
	std::vector<int> quality;//
	quality.push_back(IMWRITE_JPEG_QUALITY);//实现图像的JPEG质量等级压缩
	quality.push_back(30);//进行压缩, orgin=100 
	imencode(".jpg", image, data_encode, quality);//图像编码
	
	int nSize = data_encode.size();//编码尺寸
	printf("size: %ld \r\n", nSize); 
	if(nSize > 65535){
		printf("error: image size is too large\r\n");
		return -1;
	}
	for (int i = 0; i < nSize; i++)
	{
		encodeImg[i] = data_encode[i];//赋值
	}	
	
	ret=sendto(client_sockfd, (char *)(&encodeImg), nSize, 0, (struct sockaddr*)&server_addr, val);  
	if(ret<0)    
	{    
		perror("sendto failed");    
		return -1;
	}    	
	
	return 1;	
}
//

//
int SocketTransmissionClient::Transmit_Server_2(void)
{
	int ret;
	socklen_t val = sizeof(struct sockaddr);
	char sbuf[100]={0};//
	char buf[100];
	static int m=0;
	static struct timeval tv;
	gettimeofday(&tv, NULL);//

	//消息包结构    
	mavlink_message_t MSG;//调用mavlink_msg_heartbeat_pack函数将数据封包,传入到消息结构中
	uint8_t  MAVLink_Buf[518];//临时缓存区   
	uint16_t len;
/*
len = mavlink_msg_heartbeat_pack
(                      
0x01, //system_id 
0x01, //component_id
&MSG, //msg

0x02, //type
0x03, //autopilot
0x51, //base_mode
0x00, //custom_mode
0x01);//system_status
*/
/*
len = mavlink_msg_attitude_pack
(                      
0x01, //system_id 
0x01, //component_id
&MSG, //msg

0x00, //time_boot_ms
2.33, //roll
5.45, //pitch
7.56, //yaw
2.0, //speed r
6.0, //p
9.0);//y
*/
/*
len = mavlink_msg_scaled_imu_pack
(                      
0x01, //system_id 
0x01, //component_id
&MSG, //msg

0x00, //time_boot_ms
10, //roll
20, //
30, //
-10, // 
-20, //
-30, //
40, //
50, //
60);//
*/
len = mavlink_msg_gps_state_pack
(                      
0x01, //system_id 
0x01, //component_id
&MSG, //msg

0x02, //type
23.30046611+m*0.0001, //lat
115.9164699287, //lon
0x03);//satellite_num
//115.91646992872117,23.30046611167924
len = mavlink_msg_to_send_buffer(MAVLink_Buf, &MSG);//调用mavlink_msg_to_send_buffer函数将消息结构里面的数据以字节流的方式保存到缓存区
ret=sendto(client_sockfd, (char *)(&MAVLink_Buf), len, 0, (struct sockaddr*)&server_addr, val);//发送缓存区里面的数据 


	//while(1);
	cin>>sbuf;
	//sbuf[0]='1';
	//sbuf[1400]='1';
	gettimeofday(&tv, NULL);//
	ret=sendto(client_sockfd, (char *)(&sbuf), 5, 0, (struct sockaddr*)&server_addr, val);  
	if(ret<0)    
	{    
		perror("sendto failed");    
		return -1;
	}    	
	
	printf("ms: %d-%ld\n", m, tv.tv_sec*1000 + tv.tv_usec/1000);  //ms
	//printf("us: %d, %ld\n", m, tv.tv_sec*1000000 + tv.tv_usec);  //微秒
	m++;
	//waitKey(500);//等待(ms)	
	//waitKey(500);//等待(ms)	
	
	//return 1;
	
	
	return 1;	
}


int SocketTransmissionClient::Recv_Server(void)
{
	int ret, i; 
	socklen_t val = sizeof(struct sockaddr);
	unsigned char rbuf[1000];

	static int m=0;
	static mavlink_message_t mavlMsg;
	static mavlink_status_t  _status;
	mavlink_set_pid_speed_t current_pid_speed;//

	//static struct timeval tv;
	//gettimeofday(&tv, NULL);//
	
	//接收发来的消息   
	//puts("waiting data ......");    
        ret=recvfrom(client_sockfd, rbuf, sizeof(rbuf), 0, (struct sockaddr*)&server_addr, &val);  
	if(ret<0)    
	{    
		//perror("recvfrom failed");  
		printf("recvfrom failed");   
		//return -1;
	}    
	//gettimeofday(&tv, NULL);//
	//printf("r: %d\n", ret);  
	//printf("ms: %d-%ld\n", m, tv.tv_sec*1000 + tv.tv_usec/1000);  //ms
	//printf("us:%d, %ld\n", m, tv.tv_sec*1000000 + tv.tv_usec);  //微秒
	 
	//for(i=0; i<ret; i++)
	//{
	//   printf("%x ", rbuf[i]);	
	//}
        for(i=0; i<ret; i++)
        {
            if(mavlink_parse_char(1, rbuf[i],
               &mavlMsg, &_status))//解析出完整的一帧mavlink数据
            {
		printf("gid-%d\n", mavlMsg.msgid);
               switch(mavlMsg.msgid)
               {
                 //case MAVLINK_MSG_ID_HEARTBEAT:
                 //mavlink_msg_heartbeat_decode(&mavlMsg,
                 //        &current_heartbeat);//解码心跳包
		 case MAVLINK_MSG_ID_SET_PID_SPEED:
		 mavlink_msg_set_pid_speed_decode(&mavlMsg,
			&current_pid_speed);
		 break;	
	       }
		printf("ps:%d %d %d %d %d %d %d %d %d\n",
		current_pid_speed.rol_pid_p, current_pid_speed.rol_pid_i,
		current_pid_speed.rol_pid_d, current_pid_speed.pit_pid_p,
		current_pid_speed.pit_pid_i, current_pid_speed.pit_pid_d,
		current_pid_speed.yaw_pid_p, current_pid_speed.yaw_pid_i,
		current_pid_speed.yaw_pid_d);

            }
        }


	cout<<"m-"<<m<<endl;
	m++;

	//printf("the data :%s\n", rbuf); 
	//bzero(rbuf, sizeof(rbuf));			
	
	return 1;
}







